
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_state.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"
#include "define.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/board_config.h>
#include <internal_error/internal_error.h>
#include <battery_monitor/battery_monitor.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static MAV_STATE system_status();
static MAV_TYPE frame_type();
static MAV_MODE base_mode();
/*----------------------------------variable----------------------------------*/
static uitc_vehicle_hearbeat vehicle_hearbeat;
static uitc_vehicle_status vehicle_status;
static uitc_vehicle_vfr_hud vehicle_vfr_hud;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool fms_any_failsafe_triggered()
{
    return fms.failsafe.state != FAILSAFE_NONE || battmonitor_has_failsafed() || fms.failsafe.adsb;
}

void fms_publish_vehicle_hearbeat()
{
    vehicle_hearbeat.timestamp_us = time_micros64();

    vehicle_hearbeat.custom_mode = (uint32_t)mode_number(fms.control_mode);
    vehicle_hearbeat.type = frame_type();
    vehicle_hearbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
    vehicle_hearbeat.base_mode = base_mode();
    vehicle_hearbeat.system_status = system_status();

    itc_publish(ITC_ID(vehicle_hearbeat), &vehicle_hearbeat);
}

void fms_publish_vehicle_vfr_hud()
{
    vehicle_vfr_hud.timestamp_us = time_micros64();

    uitc_vehicle_local_position local_position;
    itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position);

    uitc_vehicle_attitude vehicle_attitude;
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);

    uitc_vehicle_status vehicle_status;
    itc_copy_from_hub(ITC_ID(vehicle_status), &vehicle_status);

    if (!ahrs_airspeed_estimate(fms.ahrs, &vehicle_vfr_hud.airspeed)) {
        vehicle_vfr_hud.airspeed = fms.ahrs->gps_ground_speed;
    }

    vehicle_vfr_hud.groundspeed = sqrtf(local_position.vx*local_position.vx+local_position.vy*local_position.vy);
    vehicle_vfr_hud.alt         = -local_position.z - vehicle_status.arming_altitude_m;
    vehicle_vfr_hud.climb       = -local_position.vz;
    vehicle_vfr_hud.heading     = (int16_t)math_wrap_360(vehicle_attitude.vehicle_euler.yaw*RAD_TO_DEG);
    vehicle_vfr_hud.throttle    = fms_throttle_percentage();

    itc_publish(ITC_ID(vehicle_vfr_hud), &vehicle_vfr_hud);
}

/**
  * @brief       
  * @param[in]   b  
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_actuator_armed_notify(bool b)
{
    notify_flags.armed = b;

    vehicle_status.timestamp_us = time_micros64();
    vehicle_status.armed = b;
    vehicle_status.arming_altitude_m = fms.ahrs->arming_altitude_m;
    vehicle_status.vehicle_type = frame_type() == MAV_TYPE_FIXED_WING ? UITC_VEHICLE_STATUS_VEHICLE_TYPE_FIXED_WING : UITC_VEHICLE_STATUS_VEHICLE_TYPE_VTOL;
    itc_publish(ITC_ID(vehicle_status), &vehicle_status);
}

static MAV_STATE vehicle_system_status()
{
    if (fms.control_mode == (mode_base_t)&fms.mode_initializing) {
        return MAV_STATE_CALIBRATING;
    }
    if (fms_any_failsafe_triggered()) {
        return MAV_STATE_CRITICAL;
    }
    if (fms.crash_state.is_crashed) {
        return MAV_STATE_EMERGENCY;
    }
    if (fms_is_flying()) {
        return MAV_STATE_ACTIVE;
    }

    return MAV_STATE_STANDBY;
}

static MAV_STATE system_status()
{
    MAV_STATE _system_status = vehicle_system_status();
    if (_system_status < MAV_STATE_CRITICAL) {
        // note that POWEROFF and FLIGHT_TERMINATION are both >
        // CRITICAL, so we will not overwrite POWEROFF and
        // FLIGHT_TERMINATION even if we have internal errors.  If new
        // enum entries are added then this may also not overwrite
        // those.
        if (internal_error_get_singleton()->internal_errors) {
            _system_status = MAV_STATE_CRITICAL;
        }
    }
    return _system_status;
}

static MAV_TYPE frame_type()
{
#if HAL_QUADPLANE_ENABLED
    return plane.quadplane.get_mav_type();
#else
    return MAV_TYPE_FIXED_WING;
#endif
}

static MAV_MODE base_mode()
{
    uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    // work out the base_mode. This value is not very useful
    // for APM, but we calculate it as best we can so a generic
    // MAVLink enabled ground station can work out something about
    // what the MAV is up to. The actual bit values are highly
    // ambiguous for most of the APM flight modes. In practice, you
    // only get useful information from the custom_mode, which maps to
    // the APM flight mode and has a well defined meaning in the
    // ArduPlane documentation
    switch (mode_number(fms.control_mode )) {
    case MN_MANUAL:
    case MN_TRAINING:
    case MN_ACRO:
#if HAL_QUADPLANE_ENABLED
    case MN_QACRO:
        _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
        break;
#endif
    case MN_STABILIZE:
    case MN_FLY_BY_WIRE_A:
    case MN_AUTOTUNE:
    case MN_FLY_BY_WIRE_B:
#if HAL_QUADPLANE_ENABLED
    case MN_QSTABILIZE:
    case MN_QHOVER:
    case MN_QLOITER:
    case MN_QLAND:
#if QAUTOTUNE_ENABLED
    case MN_QAUTOTUNE:
#endif
#endif  // HAL_QUADPLANE_ENABLED
    case MN_CRUISE:
        _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
        break;
    case MN_AUTO:
    case MN_RTL:
    case MN_LOITER:
    case MN_THERMAL:
    case MN_AVOID_ADSB:
    case MN_GUIDED:
    case MN_CIRCLE:
    case MN_TAKEOFF:
#if HAL_QUADPLANE_ENABLED
    case MN_QRTL:
    case MN_LOITER_ALT_QLAND:
#endif
        _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
                     MAV_MODE_FLAG_STABILIZE_ENABLED;
        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
        // APM does in any mode, as that is defined as "system finds its own goal
        // positions", which APM does not currently do
        break;
    case MN_INITIALISING:
        break;
    }

    if (!fms.training_manual_pitch || !fms.training_manual_roll) {
        _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;        
    }

    if (fms.control_mode != (mode_base_t)&fms.mode_manual && fms.control_mode != (mode_base_t)&fms.mode_initializing) {
        // stabiliser of some form is enabled
        _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
    }

    if (fms.g.stick_mixing != SM_NONE && fms.control_mode != (mode_base_t)&fms.mode_initializing) {
        if ((fms.g.stick_mixing != SM_VTOL_YAW) || (fms.control_mode == (mode_base_t)&fms.mode_auto)) {
            // all modes except INITIALISING have some form of manual
            // override if stick mixing is enabled
            _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
        }
    }

    // we are armed if we are not initialising
    if (fms.control_mode != (mode_base_t)&fms.mode_initializing && mb_arming_is_armed()) {
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    }

    // indicate we have set a custom mode
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    return (MAV_MODE)_base_mode;
}

/*------------------------------------test------------------------------------*/


